﻿using CG5_KT.Utils;
using System;
using System.Globalization;
using System.Runtime.InteropServices;

namespace CG5_KT.Primitives
{
    public struct Matrix
    {
        public static readonly Matrix Identity = new Matrix() { M11 = 1.0f, M22 = 1.0f, M33 = 1.0f, M44 = 1.0f };

        public float M11;
        public float M12;
        public float M13;
        public float M14;
        public float M21;
        public float M22;
        public float M23;
        public float M24;
        public float M31;
        public float M32;
        public float M33;
        public float M34;
        public float M41;
        public float M42;
        public float M43;
        public float M44;

        public static void Multiply(ref Matrix left, ref Matrix right, out Matrix result)
        {
            Matrix temp = new Matrix();
            temp.M11 = (left.M11 * right.M11) + (left.M12 * right.M21) + (left.M13 * right.M31) + (left.M14 * right.M41);
            temp.M12 = (left.M11 * right.M12) + (left.M12 * right.M22) + (left.M13 * right.M32) + (left.M14 * right.M42);
            temp.M13 = (left.M11 * right.M13) + (left.M12 * right.M23) + (left.M13 * right.M33) + (left.M14 * right.M43);
            temp.M14 = (left.M11 * right.M14) + (left.M12 * right.M24) + (left.M13 * right.M34) + (left.M14 * right.M44);
            temp.M21 = (left.M21 * right.M11) + (left.M22 * right.M21) + (left.M23 * right.M31) + (left.M24 * right.M41);
            temp.M22 = (left.M21 * right.M12) + (left.M22 * right.M22) + (left.M23 * right.M32) + (left.M24 * right.M42);
            temp.M23 = (left.M21 * right.M13) + (left.M22 * right.M23) + (left.M23 * right.M33) + (left.M24 * right.M43);
            temp.M24 = (left.M21 * right.M14) + (left.M22 * right.M24) + (left.M23 * right.M34) + (left.M24 * right.M44);
            temp.M31 = (left.M31 * right.M11) + (left.M32 * right.M21) + (left.M33 * right.M31) + (left.M34 * right.M41);
            temp.M32 = (left.M31 * right.M12) + (left.M32 * right.M22) + (left.M33 * right.M32) + (left.M34 * right.M42);
            temp.M33 = (left.M31 * right.M13) + (left.M32 * right.M23) + (left.M33 * right.M33) + (left.M34 * right.M43);
            temp.M34 = (left.M31 * right.M14) + (left.M32 * right.M24) + (left.M33 * right.M34) + (left.M34 * right.M44);
            temp.M41 = (left.M41 * right.M11) + (left.M42 * right.M21) + (left.M43 * right.M31) + (left.M44 * right.M41);
            temp.M42 = (left.M41 * right.M12) + (left.M42 * right.M22) + (left.M43 * right.M32) + (left.M44 * right.M42);
            temp.M43 = (left.M41 * right.M13) + (left.M42 * right.M23) + (left.M43 * right.M33) + (left.M44 * right.M43);
            temp.M44 = (left.M41 * right.M14) + (left.M42 * right.M24) + (left.M43 * right.M34) + (left.M44 * right.M44);
            result = temp;
        }

        public static Matrix MakeViewMatrix(Vector3 cPos, Vector3 cTarget, Vector3 cUp)
        {
            Matrix resultM = new Matrix();

            Vector3 cZ = cPos - cTarget;
            cZ.Normalize();

            Vector3 cX = Vector3.Cross(cUp, cZ);
            cX.Normalize();

            Vector3 cY = Vector3.Cross(cZ, cX);
            cY.Normalize();

            resultM = Matrix.Identity;
            resultM.M11 = cX.X; resultM.M21 = cX.Y; resultM.M31 = cX.Z;
            resultM.M12 = cY.X; resultM.M22 = cY.Y; resultM.M32 = cY.Z;
            resultM.M13 = cZ.X; resultM.M23 = cZ.Y; resultM.M33 = cZ.Z;

            resultM.M41 = Vector3.Dot(cX, cPos);
            resultM.M42 = Vector3.Dot(cY, cPos);
            resultM.M43 = Vector3.Dot(cZ, cPos);

            resultM.M41 = -resultM.M41;
            resultM.M42 = -resultM.M42;
            resultM.M43 = -resultM.M43;

            return resultM;
        }

        public static Matrix MakePerspectiveMatrix(float fov, float aspect, float znear, float zfar)
        {
            float yScale = (float)(1.0 / Math.Tan(fov * 0.5f));
            float xScale = yScale / aspect;

            float halfWidth = znear / xScale;
            float halfHeight = znear / yScale;

            return PerspectiveOffCenter(-halfWidth, halfWidth, -halfHeight, halfHeight, znear, zfar);
        }

        public static Matrix PerspectiveOffCenter(float left, float right, float bottom, float top, float znear, float zfar)
        {
            float zRange = zfar / (zfar - znear);

            var result = new Matrix();
            result.M11 = 2.0f * znear / (right - left);
            result.M22 = 2.0f * znear / (top - bottom);
            result.M31 = (left + right) / (left - right);
            result.M32 = (top + bottom) / (bottom - top);
            result.M33 = zRange;
            result.M34 = 1.0f;
            result.M43 = -znear * zRange;

            result.M31 *= -1.0f;
            result.M32 *= -1.0f;
            result.M33 *= -1.0f;
            result.M34 *= -1.0f;

            return result;
        }

        public static Matrix Translation(Vector3 value)
        {
            var result = Matrix.Identity;
            result.M41 = value.X;
            result.M42 = value.Y;
            result.M43 = value.Z;

            return result;
        }


        public static Matrix operator *(Matrix left, Matrix right)
        {
            Matrix result;
            Multiply(ref left, ref right, out result);
            return result;
        }

        public static Matrix RotationYawPitchRoll(float yaw, float pitch, float roll)
        {
            Quaternion quaternion = Quaternion.RotationYawPitchRoll(yaw, pitch, roll);

            return RotationQuaternion(quaternion);
        }

        public static Matrix RotationQuaternion(Quaternion rotation)
        {
            float xx = rotation.X * rotation.X;
            float yy = rotation.Y * rotation.Y;
            float zz = rotation.Z * rotation.Z;
            float xy = rotation.X * rotation.Y;
            float zw = rotation.Z * rotation.W;
            float zx = rotation.Z * rotation.X;
            float yw = rotation.Y * rotation.W;
            float yz = rotation.Y * rotation.Z;
            float xw = rotation.X * rotation.W;

            Matrix result = Matrix.Identity;
            result.M11 = 1.0f - (2.0f * (yy + zz));
            result.M12 = 2.0f * (xy + zw);
            result.M13 = 2.0f * (zx - yw);
            result.M21 = 2.0f * (xy - zw);
            result.M22 = 1.0f - (2.0f * (zz + xx));
            result.M23 = 2.0f * (yz + xw);
            result.M31 = 2.0f * (zx + yw);
            result.M32 = 2.0f * (yz - xw);
            result.M33 = 1.0f - (2.0f * (yy + xx));
            return result;
        }
    }
}
